import pandas as pds
import numpy as np

import matplotlib.patches as mpatches
from matplotlib import pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei']  # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus'] = False  # 用来正常显示负号


def draw_curve(df,all_tit,xlabel,font_size=20):
    #color_linestyle = ['g--','b-.','r-']
    #labels = ["Classic method","LM method","Our method"]
    y = ['c1','c2','c3','o1','o2','o3','l1','l2','l3']
    titles = [r"S\m",r"$\Delta_{c}$\m",r"$\Delta_{R}$\m"]
    label = ['(a)','(b)','(c)','(d)','(e)','(f)','(g)','(h)','(i)']
    fig, ax = plt.subplots(3, 2,figsize=(24, 24))
    #fig, ax = plt.subplots(3, 2)
    plt.subplots_adjust(wspace = 0.3, hspace =0.2)
    plt.tick_params(labelsize=int(font_size*0.8))
    i = 0
    x = df['x']
    for title in titles:
        ax[i][0].plot(x, df[y[i]], 'g-',linewidth=1, label="Classic method")
        ax[i][0].plot(x, df[y[i+6]], 'b-.',linewidth=1, label="LM method")
        ax[i][0].plot(x, df[y[i+3]], 'r--',linewidth=1, label="Our method")
        ax[i][1].plot(x[:90], df[y[i]][:90], 'g-',linewidth=1, label="Classic method")
        ax[i][1].plot(x[:90], df[y[i+6]][:90], 'b-.',linewidth=1, label="LM method")
        ax[i][1].plot(x[:90], df[y[i+3]][:90], 'r--',linewidth=1, label="Our method")
        ax[i][0].set_xlabel(xlabel+"\n"+label[i]+all_tit[0],fontsize=int(font_size*0.8))
        ax[i][0].set_ylabel(title,fontsize=int(font_size*0.8))
        ax[i][1].set_xlabel(xlabel+"\n"+label[i+3]+all_tit[1],fontsize=int(font_size*0.8))
        #ax[i][0].set_title(title)
        ax[i][1].tick_params(labelsize=int(font_size*0.8))
        ax[i][0].tick_params(labelsize=int(font_size*0.8))
        ax[i][0].legend(fontsize=font_size)
        ax[i][1].legend(fontsize=font_size)
        i = i + 1
    fig.suptitle(all_tit[0])
    plt.savefig('arclen.jpg')
    #plt.show()


def draw_curve1(df1,df2,all_tit,xlabel,font_size=20):
    #color_linestyle = ['g--','b-.','r-']
    #labels = ["Classic method","LM method","Our method"]
    
    y = ['c1','c2','c3','o1','o2','o3','l1','l2','l3']
    titles = [r"S/(m)",r"$\Delta_{c}$/(m)",r"$\Delta_{R}$/(m)"]
    label = ['(a)','(b)','(c)','(a)','(b)','(c)','(g)','(h)','(i)']
    #fig, ax = plt.subplots(2,2,figsize=(24, 24))
    fig, ax = plt.subplots(3, 2,figsize=(24, 24))
    plt.subplots_adjust(wspace = 0.2, hspace =0.2)
    
    i = 0
    for title in titles:
        ax[i][0].plot(df1['x'], df1[y[i]], 'g-',linewidth=1, label="Classic method")
        ax[i][0].plot(df1['x'], df1[y[i+6]], 'b-.',linewidth=1, label="LM method")
        ax[i][0].plot(df1['x'], df1[y[i+3]], 'r--',linewidth=1, label="Our method")
        ax[i][1].plot(df2['x'], df2[y[i]], 'g-',linewidth=1, label="Classic method")
        ax[i][1].plot(df2['x'], df2[y[i+6]], 'b-.',linewidth=1, label="LM method")
        ax[i][1].plot(df2['x'], df2[y[i+3]], 'r--',linewidth=1, label="Our method")
        ax[i][0].set_xlabel(xlabel[0]+"\n"+label[i]+all_tit[0],fontsize=int(font_size*0.8))
        ax[i][0].set_ylabel(title,fontsize=int(font_size*0.8))
        ax[i][1].set_xlabel(xlabel[1]+"\n"+label[i+3]+all_tit[1],fontsize=int(font_size*0.8))
        ax[i][1].set_ylabel(title,fontsize=int(font_size*0.8))
        #ax[i][0].set_title(title)W
        ax[i][1].tick_params(labelsize=int(font_size*0.8))
        ax[i][0].tick_params(labelsize=int(font_size*0.8))
        ax[i][0].legend(fontsize=font_size)
        ax[i][1].legend(fontsize=font_size)
        i = i + 1
    fig.suptitle(all_tit[0])
    plt.savefig('arcwidth.jpg')
    #plt.show()

def draw_curve2(df):
    #color_linestyle = ['g--','b-.','r-']
    #labels = ["Classic method","LM method","Our method"]
    y = ['c','o1','o2','o3','l1','l2','l3']
    titles = [r"S/(m)",r"$\Delta_{c}$/(m)",r"$\Delta_{R}$/(m)"]
    label = ['(a)','(b)','(c)','(d)','(e)','(f)','(g)','(h)','(i)']
    #plt.subplots_adjust(wspace = 0.1, hspace =0.32)
    #i = 0
    plt.plot(df['x'], df['c'], 'g-',linewidth=1, label="Classic method")
    plt.plot(df['x'], df['l'], 'b-.',linewidth=1, label="LM method")
    plt.plot(df['x'], df["o"], 'r--',linewidth=1, label="Our method")
   
    plt.xlabel("点个数"+"\n"+r"$\theta=45^{\circ}$ $\epsilon=0.0$ $\omega=0.03$")
    plt.ylabel(u"时间/(ms)")
    #ax[i][0].set_title(title)
    plt.legend(fontsize=20)
    #plt.legend(fontsize='small')
    plt.show()
#D:\Program\CAD_Program\地铁中轴线绘制程序\北新泾~威宁路里程统计表 - 副本.xlsxplot\experiment_data.xlsx
xls = pds.ExcelFile('D:\\Program\\circle_fit\\plot\\experiment_data.xlsx')
def arc_len():
    sheet1 = xls.parse(0,header=0)
    hd_title = [r"$\omega=0.05$ $\epsilon=0.0$ $\theta \in [1^{\circ}, 360^{\circ}]$",r"$\omega=0.05$ $\epsilon=0.0$ $\theta \in [1^{\circ}, 90^{\circ}]$"]
    draw_curve(sheet1,hd_title,r"$\theta$($^{\circ}$)")
def arc_width():
    sheet2 = xls.parse(2,header=0)
    sheet3 = xls.parse(5,header=0)
    hd_title = [r"$\theta=72^{\circ}$ $\epsilon=0.0$ $\omega \in [0.0001, 0.18]$",r"$\theta=45^{\circ}$ $\omega=0.05$ $\epsilon \in[-0.06, 0.06]$"]
    draw_curve1(sheet2,sheet3,hd_title,[r"$\omega$",r'$(1+\epsilon)R$/(m)'])
def arc_speed():
    sheet4 = xls.parse(4,header=0)
    hd_title = [r"$\theta=72^{\circ}$ $\epsilon=0.0$ $\omega=0.03$",r"$\theta=45^{\circ}$ $\epsilon=0.5$ $\epsilon \in[-0.06, 0.06]$"]
    draw_curve2(sheet4)

#arc_width()
def arc_patch(center, radius, theta, ax=None, resolution=100, **kwargs):
    # make sure ax is not empty
    if ax is None:
        ax = plt.gca()
    # generate the points
    theta = np.linspace(np.radians(theta[0]), np.radians(theta[1]), resolution)
    #print(theta,radius)
    points0 = np.vstack((radius[0]*np.cos(theta) + center[0], 
                        radius[0]*np.sin(theta) + center[1]))
    points1 = np.vstack((radius[1]*np.cos(theta[::-1]) + center[0], 
                        radius[1]*np.sin(theta[::-1]) + center[1]))
    points = np.concatenate((points0,points1),axis=-1) 
    #print(points0[:][0])
    # build the polygon and add it to the axes
    poly = mpatches.Polygon(points.T, closed=True, **kwargs)
    ax.add_patch(poly)
    return points0[:,0],points0[:,-1],points1[:,0],points1[:,-1]

def annotate_dim(ax,xyfrom,xyto,text=None,arrowprops=None,ro=180):
    if arrowprops is None:
        arrowprops = dict(arrowstyle='<->',connectionstyle="arc3,rad=-0.4")
    if text is None:
        text = str(np.sqrt( (xyfrom[0]-xyto[0])**2 + (xyfrom[1]-xyto[1])**2 ))
    ax.annotate("",xyfrom,xyto,arrowprops=arrowprops,ha='center', va='center')
    p1 = ax.transData.transform_point((xyfrom[0], xyfrom[1]))
    p2 = ax.transData.transform_point((xyto[0], xyto[1]))
    dy = (p2[1] - p1[1])
    dx = (p2[0] - p1[0])
    rotate_ang = np.degrees(np.arctan2(dy, dx))
    xy_text = ((xyto[0]+xyfrom[0])/2.,(xyto[1]+xyfrom[1])/2.)
    ax.annotate(text,xy_text,rotation=rotate_ang+ro,ha='center', va='bottom')
    #ax.line2D( X, Y*0.2, color = 'b', linewidth=2, linestyle="--" )
    #ax.plot( X, Y*0.2, color = 'b', linewidth=2, linestyle="--" )
    #ax.annotate('-',xyto,rotation=rotate_ang+90,fontsize=16,ha='center', va='center')
#def dim_dashline(xyfrom, xyto,offset):

def draw_fit_circle(pnts_df,cirparam_df,gt_circle,ax,f_index,element=True):
    y = ['弧长','点宽度','待点个数','起点坐标','cparam','oparam','lparam']
    gt_cenx,gt_ceny,gt_r = gt_circle
    arc = cirparam_df[y[0]][0]
    width = cirparam_df[y[1]][0]
    num = int(cirparam_df[y[2]][0])
    #print(cirparam_df[y[3]])
    end = (cirparam_df[y[3]][0]-gt_circle[2])/gt_circle[2]
    title = r"{}$\theta$={}$^{}$ $\omega$={:.3f} n={} $\epsilon$={:.2f}".format(f_index,int(arc),r'\circ',width,num,end)
    ax.scatter(pnts_df["x"],pnts_df["y"],s=0.1,marker ='.',color="black",alpha=0.5)
    theta = [0,arc]
    rads = [gt_r*(1-width),gt_r*(1+width)]
    points = arc_patch([gt_cenx,gt_ceny],rads,theta, ax=ax, fill=False, color='black',linestyle="--")
    #print(points)
    circle1 = plot_circle(ax,[gt_cenx,gt_ceny],gt_r,color="black",linewidth=1,linestyle='-',label="gt_circle")
    circle2 = plot_circle(ax,cirparam_df['cparam'][0:2][::-1],cirparam_df['cparam'][2],color="g",linewidth=1,linestyle='-.',label="Classic method")
    circle3 = plot_circle(ax,cirparam_df['lparam'][0:2][::-1],cirparam_df['lparam'][2],color="b",linewidth=1,linestyle='--',label="LM method")
    circle4 = plot_circle(ax,cirparam_df['oparam'][0:2][::-1],cirparam_df['oparam'][2],color="r",linewidth=1,linestyle=':',label="Our method")
    ax.set_xlim((-12, 12))
    ax.set_ylim((-12, 12))
    #半径线
    if element:
        #ax.plot([gt_cenx,gt_ceny], [gt_cenx-gt_r, gt_ceny], color = 'black', linewidth=2, linestyle="--" )
        ax.scatter(gt_cenx,gt_ceny,s=100,c='black',marker ='+')
        cen_text = r"(x$^{g}_{c}$,y$^{g}_{c}$)"
        #ax.annotate(cen_text,[gt_cenx,gt_ceny],fontsize=10,ha='center', va='top')
        ax.annotate(cen_text,xy=(gt_cenx,gt_ceny), xycoords='data',xytext=(-20, -20),textcoords='offset points',arrowprops=dict(arrowstyle="->"))
        ax.scatter(cirparam_df[y[3]][0],gt_ceny,s=100,c='black',marker ='x')
        stpnt_text = u'约束端点\n'+r"(x$^{g}_{c}$$\plus$(1+$\epsilon$)R$^{g}_{c}$,y$^{g}_{c}$)"
        #ax.annotate(stpnt_text,(cirparam_df[y[3]][0],gt_ceny),fontsize=10,ha='center', va='top')
        ax.annotate(stpnt_text,xy=(cirparam_df[y[3]][0], 0.0), xycoords='data',xytext=(-70, -230),textcoords='offset points',arrowprops=dict(arrowstyle="->"))
        r_text = r"R$^{}_{}$={:.1f}".format('g','c',gt_r)
        annotate_dim(ax,[gt_cenx-gt_r, gt_ceny],[gt_cenx,gt_ceny],r_text,arrowprops = dict(arrowstyle='->'),ro=0)
        w1_text = r"(1+$\omega$)R$^{g}_{c}$"
        w0_text = r"(1-$\omega$)R$^{g}_{c}$"
        annotate_dim(ax,points[2],[gt_cenx,gt_ceny],w1_text,arrowprops = dict(arrowstyle='->'))
        annotate_dim(ax,points[0],[gt_cenx,gt_ceny],w0_text,arrowprops = dict(arrowstyle='->'))
        annotate_dim(ax,points[0]/3.,points[1]/3.,r"$\theta$")
    #annotate_dim(ax,r'2$\omega$R')
    ax.set_xlabel(r"X/(m)"+"\n"+title)
    ax.set_ylabel(r"Y/(m)")
    ax.legend([circle1,circle2,circle3,circle4],["gt_circle","Classic method","LM method","Our method"],fontsize='small')

def plot_circle(ax,cen,rad,**kwags):
    circle = plt.Circle(cen,rad,fill=False,**kwags)
    ax.add_artist(circle)
    return circle

def circle_compare():
    xls = pds.ExcelFile('plot\circle_draw.xlsx')
    xls1 = pds.ExcelFile('plot\experiment_data.xlsx')
    plt.rcParams.update({'font.size': 20})
    fig, ax = plt.subplots(2,2,figsize=(24, 24))
    plt.subplots_adjust(wspace = 0.15, hspace =0.15)
    f_indexs = ['(a)','(b)','(c)','(d)','(e)','(f)','(g)','(h)','(i)']
    i = 0
    for i in range(3):
        point_sheet = xls.parse(i*2,header=0)
        param_sheet = xls.parse(i*2+1,header=0)
        draw_fit_circle(point_sheet,param_sheet,[0.0,0.0,10.0],ax[int(i/2)][int(i%2)],f_indexs[i])
    sheet4 = xls1.parse(4,header=0)
    y = ['c','o1','o2','o3','l1','l2','l3']
    titles = [r"S/(m)",r"$\Delta_{c}$/(m)",r"$\Delta_{R}$/(m)"]
    
    #plt.subplots_adjust(wspace = 0.1, hspace =0.32)
    #i = 0
    ax[1][1].plot(sheet4['x'], sheet4['c'], 'g-',linewidth=1, label="Classic method")
    ax[1][1].plot(sheet4['x'], sheet4['l'], 'b-.',linewidth=1, label="LM method")
    ax[1][1].plot(sheet4['x'], sheet4["o"], 'r--',linewidth=1, label="Our method")
    ax[1][1].set_xlabel("点个数"+"\n"+r"(d)$\theta=45^{\circ}$ $\epsilon=0.0$ $\omega=0.03$")
    ax[1][1].set_ylabel(u"时间/(ms)")
    #ax[i][0].set_title(title)
    ax[1][1].legend(fontsize='small')
    
    plt.savefig('test2.jpg')
    #plt.show()

#圆弧算法效果和算法运算时间图
#circle_compare()           
#弧长比较图
arc_len()
#宽度起始点比较图
arc_width()